/*
 *  ClusterBasedMPOControlArchitecture.cpp
 *  roborobo-online
 *
 *  Created by Nicolas on 27/05/10.
 *
 */

//TODO: replace setcandidatefiness with variant from populationcontrolarchitecture


#include "ClusterBasedMPOControlArchitecture.h"

#include "ChangeDetection/include/ChangeDetectionAgentWorldModel.h"//included for being able to access the values of the changedetectionagentworldmodel in this class
#include "Contexts/Context.h"

unsigned int ClusterBasedMPOControlArchitecture::mu = 3; // should be overwritten by properties file
double ClusterBasedMPOControlArchitecture::reEvaluationRate = 0.1; // should be overwritten by properties file
double ClusterBasedMPOControlArchitecture::crossoverRate = 1.0; // should be overwritten by properties file
double ClusterBasedMPOControlArchitecture::mutationRate = 1.0; // should be overwritten by properties file
int ClusterBasedMPOControlArchitecture::id = 0;
int ClusterBasedMPOControlArchitecture::numberOfParents = 2; // should be overwritten by properties file

ClusterBasedMPOControlArchitecture::ClusterBasedMPOControlArchitecture(RobotAgentWorldModel *__wm) :
	EvolutionaryControlArchitecture(__wm) {
	myId = ++ClusterBasedMPOControlArchitecture::id;
	//std::cerr << mu <<","<<reEvaluationRate<<","<<crossoverRate<<","<<mutationRate<<","<<numberOfParents;
}

ClusterBasedMPOControlArchitecture::~ClusterBasedMPOControlArchitecture() {
	// nothing to do.
}

ControllerPtr ClusterBasedMPOControlArchitecture::getNextCandidate() {
	updateHallsOfFame();
	/*ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);

	std::vector<int> newClusterIDs = worldModel->CManager.getContextsAsIDs();

	std::vector<int> removedIDs;
	std::vector<int> addedIDs;

	for(int i =0; (unsigned)i<newClusterIDs.size();i++){
		if(!(std::find(previousClusterIDs.begin(), previousClusterIDs.end(), newClusterIDs[i]) != previousClusterIDs.end())){
			addedIDs.push_back(newClusterIDs[i]);
		}
	}

	for(int i =0; (unsigned)i<previousClusterIDs.size();i++){
		if(!(std::find(newClusterIDs.begin(), newClusterIDs.end(), previousClusterIDs[i]) != newClusterIDs.end())){
			removedIDs.push_back(previousClusterIDs[i]);
		}
	}*/


/*	for(int i =0; (unsigned)i<newClusterIDs.size();i++){
		std::cerr<< newClusterIDs[i] << " ,";
	}
	std::cerr << endl;*/

	ControllerPtr candidate;

	// if the hall of fame is full, mutate an existing controller or re-evaluate one to create a new candidate
	if (hallOfFame.size() == mu) {
		// decide whether to re-evaluate or mutate a controller
		if (Rand::randouble() < reEvaluationRate) { // re-evaluate
			candidate = Util::binaryTournament(&hallOfFame, NULL);
			if (debug) {
				cout << "Selected re-evaluation of " << candidate->ToShortString() << endl;
			}
		} else { // mutate existing
			// parent selection
			ControllerPtr parentA = Util::binaryTournament(&hallOfFame, NULL);

			// crossover or clone
			if (Rand::randouble() < crossoverRate) {
				vector<ControllerPtr> *parents = new vector<ControllerPtr> ();
				parents->push_back(parentA);
				if (debug) {
					cout << "parentSize " << parents->size() << " " << numberOfParents << endl;
				}
				while (parents->size() < (unsigned) numberOfParents) {
					if (debug) {
						cout << parents->size() << endl;
					}
					parents->push_back(Util::binaryTournament(&hallOfFame, parents));
				}
				if (debug) {
					cout << parents->size() << endl;
				}
				// create a candidate from crossover between the parents
				candidate = parentA->crossOverWithMP(parents);

				//Controller *parentB = Util::binaryTournament(&hallOfFame, parentA);
				//candidate = parentA->crossOverWith(parentB);
				if (debug) {
					cout << "parentSize " << parents->size() << " Created " << candidate->ToShortString() << " from crossover between "
							<< parents->at(0)->ToShortString();
					for (uint i = 1; i < parents->size(); i++)
						cout << " and " << parents->at(i)->ToShortString();
					cout << endl;
				}
				parents->clear();
				delete parents;
				// System.out.println("Chose parents: " + parentA + " and " +
				// parentB);
			} else {
				candidate = parentA->Clone();
				if (debug) {
					cout << "Spawned " << candidate->ToShortString() << " from " << parentA->ToShortString() << endl;
				}
			}

			// mutation
			if (Rand::randouble() < mutationRate) {
				//cout << "Before mutation: " << candidate->ToString() << endl;
				candidate->mutate();
				//cout << "After mutation: " << candidate->ToString() << endl;
			}

			if (debug) {
				cout << "Mutated " << candidate->ToShortString() << endl;
			}
		}
	}

	// if the hall of fame is not yet full, generate a new candidate randomly
	if (hallOfFame.size() < mu) {
		//candidate = Util::createRandomGenome(_wm); //SHOULD USE MORE SPECIFIC VERSION OF THE CONTROL ARCHITECTURE, NOT THE GENERAL ONE
		candidate = this->createRandomGenome();
		if (debug) {
			cout << "Generated " << candidate->ToString() << endl;
		}
	}

	return candidate;

}

void ClusterBasedMPOControlArchitecture::updateHallsOfFame(){
	ChangeDetectionAgentWorldModel* worldModel = dynamic_cast<ChangeDetectionAgentWorldModel*> (_wm);
	//std::cerr << "oi!";
	std::vector<int> newClusterIDs = worldModel->CManager.getContextsAsIDs();
	std::vector<int> removedIDs;
	std::vector<int> addedIDs;
	std::vector<int> removedIDpositions;
	std::vector<int> addedIDpositions;

	for(int i =0; (unsigned)i<previousClusterIDs.size();i++){
		if(!(std::find(newClusterIDs.begin(), newClusterIDs.end(), previousClusterIDs[i]) != newClusterIDs.end())){
			removedIDs.push_back(previousClusterIDs[i]);
			removedIDpositions.push_back(i);
		}
	}

	for(int i =1; (unsigned)i<=removedIDpositions.size();i++){
		//cerr<<removedIDpositions[removedIDpositions.size()-i];
		//correspondingControllerArrays.erase(std::remove(correspondingControllerArrays.begin(), correspondingControllerArrays.end(), 8), correspondingControllerArrays.end());
		clusteredHallsOfFame.erase(clusteredHallsOfFame.begin()+removedIDpositions[removedIDpositions.size()-i]);
	}


	for(int i =0; (unsigned)i<newClusterIDs.size();i++){
		if(!(std::find(previousClusterIDs.begin(), previousClusterIDs.end(), newClusterIDs[i]) != previousClusterIDs.end())){
			addedIDs.push_back(newClusterIDs[i]);
			addedIDpositions.push_back(i);
		}
	}

	for(int i =0; (unsigned)i<addedIDpositions.size();i++){
		//correspondingControllerArrays.insert(correspondingControllerArrays.begin()+addedIDpositions[i],addedIDs[i]);
		vector<ControllerPtr> newhallOfFame;

		clusteredHallsOfFame.insert(clusteredHallsOfFame.begin()+addedIDpositions[i],newhallOfFame);
		//replace addedIDs[i] with correspondingControllerArrays[addedIDpositions[i]]
	}

	for(int i =0; (unsigned)i<addedIDpositions.size();i++){
		for(int j =0; (unsigned)j<addedIDpositions.size();j++){

			/*int closestID;
			if(addedIDs[j]==4){
				closestID = 8;
			}else if(addedIDs[j]==8){
				closestID = 10;
			}else{
				closestID=5;
			}*/
			int closestID = worldModel->CManager.getClosestClusterIDBasedOnSimpleComparison(addedIDs[j]);
			//std::cerr << closestID << " ";
			for (int k=0; (unsigned)k<newClusterIDs.size(); k++){
				if(newClusterIDs[k]==closestID){
					//std::cerr << k << " "<< closestID<<endl;
					clusteredHallsOfFame[addedIDpositions[j]]=clusteredHallsOfFame[k];
				}
			}
		}
	}

	//below = new, might not be well tested
	previousClusterIDs = newClusterIDs;

	int currentClusterPosition = 0;
	if(newClusterIDs.size()==0){
		return;
	}
	for(int i =0; (unsigned)i<newClusterIDs.size();i++){
		if(newClusterIDs[i] == worldModel->currentCluster){
			currentClusterPosition = i;
		}
	}
	//std::cout<< "Hall of fame position: "<< currentClusterPosition << ", Cluster position: "<< currentClusterPosition << " " ;
	hallOfFame = clusteredHallsOfFame[currentClusterPosition];
}

/*
 * I replaced the original MPO setcandidate function with this one (largely from populationcontrolarchitecture) because this function
 * also makes sure candidates are only put in the hall of fame if they are actually better than one of the previous solutions. Previously,
 * it was always added.
 */
void ClusterBasedMPOControlArchitecture::setCandidateFitness(ControllerPtr candidate, double scoredFitness) {

	// running evaluation has completed. Decide if the candidate will become the active solution.
	if (candidate->GetFitness() > -1) {
		// this is a re-evaluation, average out the previous and the new score
		if (debug) {
			cout << "This was a re-evaluation, not adding a new candidate" << endl;
		}
		candidate->SetFitness((candidate->GetFitness() + scoredFitness) / 2);
		if (debug || verbose) {
			cout << "Finished re-evaluating " << candidate->ToShortString() << " - fitness: " << scoredFitness << ", avg: " << candidate->GetFitness() << endl;
		}

	} else {
		candidate->SetFitness(scoredFitness);

		// this is a first evaluation and will need to be considered for placement as an active solution
		if (hallOfFame.size() < mu) {
			// there is no full set of active solutions yet, just add this solution
			hallOfFame.push_back(candidate);

			// send out a news item about the new active solution

			//don't do lines below, has to do with communication
            //ControllerPtr active = candidate->Clone();
			//population->setNews(active, activeSolutions.size() - 1, currentTime);

		} else {
			// there is a full list of active solutions, compete with the active solutions
			int worstIndex = -1;
			ControllerPtr worst = candidate;

			// find the worst controller in the (unsorted) list
			for (unsigned int i = 0; i < hallOfFame.size(); i++) {
				if (worst->GetFitness() > hallOfFame[i]->GetFitness()) {
					worst = hallOfFame[i];
					worstIndex = i;
				}
			}

			// if the candidate is better than the worst controller in the list, have it take that controller's place
			if (scoredFitness > worst->GetFitness()) {
				if (debug) {
					cout << "This was first evaluation, candidate became active solution" << endl;
				}
				hallOfFame[worstIndex] = candidate;

				// send out a news item about the new active solution
				//don't do lines below, has to do with communication
                //ControllerPtr active = candidate->Clone();
                //population->setNews(active, worstIndex, currentTime);

			} else {
				if (debug) {
					cout << "This was first evaluation, candidate failed" << endl;
				}
				// the candidate has lost the competition; have it deleted
				// done automatically by smart pointer
			}
		}
		// Don't print if candidate was deleted
		if (candidate && (debug || verbose)) {
			cout << "Finished evaluating " << candidate->ToShortString() << " - fitness: " << scoredFitness << endl;
		}
	}

	if (!debug && !verbose) {
		// there has been no output on the fitness yet, do that now
		//cout << scoredFitness << endl;
	}
}

/*void ClusterBasedMPOControlArchitecture::setCandidateFitness(ControllerPtr candidate, double scoredFitness) {

	// running evaluation has completed. Place candidate in hall of fame.
	if (candidate->GetFitness() > -1) {
		// this is a re-evaluation, average out the previous and the new score
		if (debug) {
			cout << "This was a re-evaluation, not adding a new candidate" << endl;
		}
		candidate->SetFitness((candidate->GetFitness() + scoredFitness) / 2);
		if (debug || verbose) {
			cout << "Finished re-evaluating " << candidate->ToShortString() << " - fitness: " << scoredFitness << ", avg: " << candidate->GetFitness() << endl;
		}
	} else {
		// this was a first evaluation, add to the bottom of the hall of fame
		if (debug) {
			cout << "This was first evaluation, adding a new candidate" << endl;
		}
		candidate->SetFitness(scoredFitness);
		hallOfFame.push_back(candidate);
		if (debug || verbose) {
			cout << "Finished evaluating " << candidate->ToShortString() << " - fitness: " << scoredFitness << endl;
		}
	}
	if (!debug && !verbose) {
		// there has been no output on the fitness yet, do that now
		cout << scoredFitness << endl;
	}

	// order the hall of fame - if there are too many entries, remove the worst-scoring one
	sort(hallOfFame.begin(), hallOfFame.begin() + hallOfFame.size(), Controller::compare);
	if (hallOfFame.size() > mu) {
		hallOfFame.pop_back();
	}
	if (debug) {
		cout << " --- HOF #: " << (rankOf(candidate) + 1) << " of " << hallOfFame.size() << endl;
	}

}*/

int ClusterBasedMPOControlArchitecture::rankOf(ControllerPtr ctrl) {
	vector<ControllerPtr>::iterator result = find(hallOfFame.begin(), hallOfFame.end(), ctrl);
	if (result == hallOfFame.end()) {
		return -1;
	}
	return result - hallOfFame.begin();
}

